#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <jsk_recognition_msgs/BoundingBox.h>
#include <jsk_recognition_msgs/BoundingBoxArray.h>

#include <fstream>
#include <iostream>
#include <string>
#include <vector>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include "common.h"


int main(int argc, char** argv)
{
    ros::init(argc, argv, "pcd_publisher");
    ros::NodeHandle nh;

    // read pcd files and corresponding label files from folder
    std::string folder_path = "/media/zwh/T7/ZWH/bags/dataset/nanchuan_det_seg";
    std::vector<std::string> pcd_files, label_files, boxes_files;


    get_alldata_path(folder_path+"/images/train", pcd_files,"pcd");
    get_alldata_path(folder_path+"/da_seg_annotations/train", label_files,"txt");
    get_alldata_path(folder_path+"/det_annotations/train", boxes_files,"txt");

    size_t len = pcd_files.size();
    assert(label_files.size()==len && boxes_files.size()==len);

//
//    for (const auto& entry : std::filesystem::directory_iterator(folder_path+"/images/train"))
//    {
//        if (entry.path().extension() == ".pcd")
//            pcd_files.push_back(entry.path());
//        else if (entry.path().extension() == ".txt")
//            label_files.push_back(entry.path());
//    }

    // publish point cloud and bounding box
    ros::Publisher pub_cloud = nh.advertise<sensor_msgs::PointCloud2>("/point_cloud", 1);
    ros::Publisher pub_label = nh.advertise<sensor_msgs::PointCloud2>("/point_label", 1);
    ros::Publisher pub_box = nh.advertise<jsk_recognition_msgs::BoundingBoxArray>("/bounding_boxes", 1);

    while (ros::ok())
    {
        for (std::size_t i = 0; i < pcd_files.size(); ++i)
        {
            // 发布原始rgba点云
            pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
            if (pcl::io::loadPCDFile<pcl::PointXYZRGB> (pcd_files[i], *cloud) == -1)
            {
                PCL_ERROR ("Couldn't read file!\n");
                return (-1);
            }
            std::cout<<"当前是:"<<pcd_files[i]<<std::endl;
            // create PointCloud2 message
            sensor_msgs::PointCloud2 cloud_msg;
            pcl::toROSMsg(*cloud, cloud_msg);
            cloud_msg.header.frame_id = "base_link";
            cloud_msg.header.stamp = ros::Time::now();

            // publish PointCloud2 message
            pub_cloud.publish(cloud_msg);

            //发布分割标注结果
            std::map<std::string,int> label;
            label.insert(std::make_pair("NO",0));
            label.insert(std::make_pair("Car",1));
            label.insert(std::make_pair("Ground",2));
            std::ifstream input(label_files[i]);
            std::string line;
            pcl::PointCloud<pcl::PointXYZI> label_pc;
            while (getline(input,line)){
                stringstream ss;
                ss.str(line);
                string single;
                vector<string> tvecData;
                while(getline(ss,single,'\t')){
                    tvecData.push_back(single);
                }
                ss.clear();
                pcl::PointXYZI pt;
                pt.x = std::atof(tvecData[1].c_str());
                pt.y = std::atof(tvecData[2].c_str());
                pt.z = std::atof(tvecData[3].c_str());
                pt.intensity = int(label[tvecData[4]]*50.0) %255;
                label_pc.push_back(pt);
            }
            assert(label_pc.size()==cloud->size());
            pcl::toROSMsg(label_pc, cloud_msg);
            cloud_msg.header.frame_id = "base_link";
            cloud_msg.header.stamp = ros::Time::now();
            // publish PointCloud2 message
            pub_label.publish(cloud_msg);


            // 发布检测标注结果
            jsk_recognition_msgs::BoundingBoxArray box_array;
            box_array.header.frame_id = "base_link";
            box_array.header.stamp = ros::Time::now();

            // read label file
            std::ifstream input2(boxes_files[i]);
            while (getline(input2,line)){
                stringstream ss;
                ss.str(line);
                string single;
                vector<string> tvecData;
                while(getline(ss,single,'\t')){
                    tvecData.push_back(single);
                }
                ss.clear();

                jsk_recognition_msgs::BoundingBox box;
                box.dimensions.x = std::atof(tvecData[2].c_str());
                box.dimensions.y = std::atof(tvecData[3].c_str());
                box.dimensions.z = std::atof(tvecData[4].c_str());
                box.pose.position.x = std::atof(tvecData[5].c_str());
                box.pose.position.y = std::atof(tvecData[6].c_str());
                box.pose.position.z = std::atof(tvecData[7].c_str());
                float yaw = std::atof(tvecData[9].c_str())*M_PI /180.0;
                box.pose.orientation.w = cos(yaw / 2.0);
                box.pose.orientation.z = sin(yaw / 2.0);

                box.header.frame_id = "base_link";
                box.header.stamp = ros::Time::now();
                box_array.boxes.push_back(box);
            }
            // publish BoundingBoxArray message
            pub_box.publish(box_array);

            // sleep for a specified time
            ros::Duration(2).sleep();
        }
    }

    return 0;
}
